#ifndef __PERCEPTION_VISUALIZATION_H__
#define __PERCEPTION_VISUALIZATION_H__

#include <ros/ros.h>
#include "core/common/core.h"
#include "core/robot_model/robot_model_base.h"
#include <sensor_msgs/PointCloud2.h>

class PerceptionVisualization{
    public:
        PerceptionVisualization();
        ~PerceptionVisualization() = default;

        // 设置机器人模型
        void setRobotModel(boost::shared_ptr<ModelBase> model_ptr);

        // 初始化深度转点云发布者
        void initDepth2PointCloudPublish(string topic_name);
        // 发布原始深度点云
        void publishDepthPointCloudRaw(sensor_msgs::PointCloud2::Ptr depth_pointcloud_ptr);
        // 发布处理后深度点云
        void publishDepthPointCloud(sensor_msgs::PointCloud2::Ptr depth_pointcloud_ptr);

        // 初始化雷达转点云发布者
        void initScan2PointCloudPublish(string topic_name);
        // 发布原始雷达点云
        void publishScanPointCloudRaw(sensor_msgs::PointCloud2::Ptr scan_pointcloud_ptr);
        // 发布处理后雷达点云
        void publishScanPointCloud(sensor_msgs::PointCloud2::Ptr scan_pointcloud_ptr);

        inline bool isDepthCameraInit() const{
            return depthcamera_init_;
        }
        inline bool isScanInit() const{
            return scan_init_;
        }

    private:
        ros::NodeHandle nh_;
        ros::Publisher depth2pointcloud_pub_;
        ros::Publisher depth2pointcloud_raw_pub_;
        ros::Publisher scan2pointcloud_pub_;
        ros::Publisher scan2pointcloud_raw_pub_;

        // 实例化
        boost::shared_ptr<ModelBase> robotModelPtr_;        //机器人模型

        bool depthcamera_init_;
        bool scan_init_;
};

#endif

